# launch/gzugv.launch.py

import os
from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import (
    DeclareLaunchArgument,
    ExecuteProcess,
    OpaqueFunction,
    IncludeLaunchDescription,
    GroupAction,
)
from launch.substitutions import (
    LaunchConfiguration,
    Command,
    FindExecutable,
    PathJoinSubstitution,
)
from launch_ros.actions import PushRosNamespace, Node
from launch_ros.substitutions import FindPackageShare


def spawn_ugv(context):
    ugv_id = LaunchConfiguration('ugv_id').perform(context)
    ugv_pose_str = LaunchConfiguration('ugv_pose').perform(context)
    model_name = LaunchConfiguration('model_name',default='rover_ackermann').perform(context) 
    pkg_sls_launch_sim = get_package_share_directory('sls_launch_sim')
    sdf_path = os.path.join(pkg_sls_launch_sim, 'models', model_name, 'model.sdf')
    if not os.path.exists(sdf_path):
        raise RuntimeError(f"SDF model not found: {sdf_path}")

    # 解析 pose
    pose = [x.strip() for x in ugv_pose_str.replace('"', '').split(',')]
    while len(pose) < 6:
        pose.append('0.0')  # 默认 z=0.5, RPY=0
    x, y, z, roll, pitch, yaw = pose[:6]

    # 使用 spawn_entity.py 插入模型
    spawn_entity = Node(
        package='ros_gz_sim',
        executable='create',
        name=f'spawn_{ugv_id}',
        arguments=[
            '-name', ugv_id,
            '-file', sdf_path,
            '-x', x, '-y', y, '-z', z,
            '-R', roll, '-P', pitch, '-Y', yaw,
            '-namespace', f'/{ugv_id}'
        ],
        output='screen'
    )

    

    return [
        GroupAction([
            PushRosNamespace(ugv_id),
            spawn_entity,

        ])
    ]


def generate_launch_description():
    return LaunchDescription([
        DeclareLaunchArgument('ugv_id', default_value='ugv1', description='Unique ID of the UGV'),
        DeclareLaunchArgument('ugv_pose', default_value='0,0,0.5,0,0,0', description='Pose: "x,y,z,roll,pitch,yaw"'),
        DeclareLaunchArgument('model_sdf', default_value='default', description='Custom SDF model file path'),

        OpaqueFunction(function=spawn_ugv),
    ])